/**
 * Copyright (C) BFH www.bfh.ch 2011
 * Code written by: Patrick Dobler, Marc Folly
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package ch.bfh.ti.kybernetik.simulator.controller.roboter;

import javax.vecmath.Vector2d;

import ch.bfh.ti.kybernetik.simulator.model.Roboter;

public final class RoboterMoveStateFactory {

	private volatile double deltaT = 0.009;

	private static RoboterMoveStateFactory roboterMoveStateFactory;

	public static RoboterMoveStateFactory getInstance() {
		if (roboterMoveStateFactory == null) {
			roboterMoveStateFactory = new RoboterMoveStateFactory();
		}
		return roboterMoveStateFactory;
	}

	private RoboterMoveStateFactory() {

	}

	public RoboterMoveState createRoboterMoveState(Roboter roboter, double roboterBottomLenght) {
		final RoboterMoveState state = new RoboterMoveState();
		state.setRoboterX(roboter.getX());
		state.setRoboterY(roboter.getY());
		state.setCurrentDirectionVector(roboter.getDirection());

		// Build Direction Vektor
		Vector2d directionVector = roboter.getDirection();
		directionVector.normalize();

		// Build Bottom Vektor and Points
		Vector2d bottomVector = new Vector2d(-directionVector.getY(), directionVector.getX());
		state.setBottomLeftPointX(roboter.getX() - (roboterBottomLenght / 2) * bottomVector.getX());
		state.setBottomLeftPointY(roboter.getY() - (roboterBottomLenght / 2) * bottomVector.getY());
		state.setBottomRightPointX(roboter.getX() + (roboterBottomLenght / 2) * bottomVector.getX());
		state.setBottomRightPointY(roboter.getY() + (roboterBottomLenght / 2) * bottomVector.getY());

		// Build Left Motor Points
		double motorSpeedLeft = roboter.getLeftMotor().getVelocity();
		state.setMotorSpeedLeftPointX(state.getBottomLeftPointX() + deltaT * motorSpeedLeft * directionVector.getX());
		state.setMotorSpeedLeftPointY(state.getBottomLeftPointY() + deltaT * motorSpeedLeft * directionVector.getY());

		// Build Right Motor Points
		double motorSpeedRight = roboter.getRightMotor().getVelocity();
		state.setMotorSpeedRightPointX(state.getBottomRightPointX() + deltaT * motorSpeedRight * directionVector.getX());
		state.setMotorSpeedRightPointY(state.getBottomRightPointY() + deltaT * motorSpeedRight * directionVector.getY());

		// Build new Roboter Position
		state.setNewRobotorPositionForwardX((state.getMotorSpeedLeftPointX() + state.getMotorSpeedRightPointX()) / 2);
		state.setNewRobotorPositionForwardY((state.getMotorSpeedLeftPointY() + state.getMotorSpeedRightPointY()) / 2);

		// Build new Roboter Direction Vektor and Point
		Vector2d topVektor = new Vector2d(state.getMotorSpeedLeftPointX(), state.getMotorSpeedLeftPointY(),
				state.getMotorSpeedRightPointX(), state.getMotorSpeedRightPointY());
		topVektor.normalize();
		Vector2d newDirectionVector = new Vector2d(topVektor.getY(), -topVektor.getX());
		state.setNewDirectionVector(newDirectionVector);

		return state;
	}

	public void setDeltaT(double deltaT) {
		this.deltaT = deltaT;
	}

	public double getDeltaT() {
		return deltaT;
	}

}
